//
// Created by LinorInk on 2024/4/23.
//

#include "servo_debug.h"

// 头尾数据为标志位
static uint16_t unpack_buffer[22] = {0xFF, 0xFF,
                                   0x00, 0x00, 0x00,
                                   0x00, 0x00, 0x00,
                                   0x00, 0x00, 0x00,
                                   0x00, 0x00, 0x00,
                                   0x00, 0x00, 0x00,
                                   0x00, 0x00, 0x00,
                                   0xFE, 0xFE};

static uint16_t servo_angles[SERVO_NUM];
static uint8_t i = 0;

taskTypedef servo_update_task;

// 数据包解析
void servo_debug_info_unpack(uint8_t data) {
    // 校验起始位
    if(i < 2) {
        if(data == unpack_buffer[i]){
            i++;
        }else {
            i = 0;
        }
        return;
    }

    if(i < 20) {
        unpack_buffer[i++] = data * 270 / 250;
        return;
    }

    if(i == 20 ){
        if( unpack_buffer[i] == data) {
            i++;
            return;
        } else {
            i = 0;
            return;
        }
    }

    // 校验结束位
    if(i == 21 && unpack_buffer[i] == data) {
        // memcpy(servo_angles, unpack_buffer + 2, SERVO_NUM * 2);
        // 解包通过后的数据更新
        for(int j = 0; j < SERVO_NUM; j++) {
            servo_setAngle(j, unpack_buffer[j + 2]);
        }
    }

    i = 0;
}

// 将数据更新到电机
void servo_debug_update() {
    // 数据解包更新
    if (!UART_isEmpty()) {
        servo_debug_info_unpack(UART_Read());
    }
}

void servo_debug_init() {
    Task_regist(&servo_update_task, 0, 0xff, servo_debug_update);
}